#include "mount_siyi.h"
#include "crc.h"
#include <cstring>
#include <cmath>
#include <chrono>


using namespace std;
using namespace cv;

#define CIRCULAR_BUFFER_SIZE		(4 * 1024)


#define MOUNT_SIYI_HEADER1       0x55    // first header byte
#define MOUNT_SIYI_HEADER2       0x66    // second header byte
#define MOUNT_SIYI_PACKETLEN_MIN 10      // minimum number of bytes in a packet.  this is a packet with no data bytes
#define MOUNT_SIYI_DATALEN_MAX   (MOUNT_SIYI_PACKETLEN_MAX-MOUNT_SIYI_PACKETLEN_MIN) // max bytes for data portion of packet
#define MOUNT_SIYI_RATE_MAX_RADS radians(90) // maximum physical rotation rate of gimbal in radans/sec
#define MOUNT_SIYI_PITCH_P       1.50    // pitch controller P gain (converts pitch angle error to target rate)
#define MOUNT_SIYI_YAW_P         1.50    // yaw controller P gain (converts yaw angle error to target rate)
#define MOUNT_SIYI_TIMEOUT_MS    1000    // timeout for health and rangefinder readings

#define MOUNT_SIYI_TCP_PORT		"37260"  // tcp test error
#define MOUNT_SIYI_UDP_PORT		"37260"

#define MOUNT_YAW_ANGLE_MIN		-160
#define MOUNT_YAW_ANGLE_MAX		160



#define MIN(a, b)   (((a) < (b)) ? (a) : (b))
#define MAX(a, b)   (((a) > (b)) ? (a) : (b))

#define ARRAY_SIZE(X) (sizeof(X)/sizeof(X[0]))
#define UINT16_VALUE(hbyte, lbyte) (static_cast<uint16_t>(((hbyte)<<8)|(lbyte)))
#define UINT32_VALUE(b3, b2, b1, b0) (static_cast<uint32_t>(((b3)<<24)|((b2)<<16)|((b1)<<8)|(b0)))

// get high or low bytes from 2 byte integer
#define LOWBYTE(i) ((uint8_t)(i))
#define HIGHBYTE(i) ((uint8_t)(((uint16_t)(i))>>8))

#define M_2PI         (M_PI * 2)


#define DEG_TO_RAD      (M_PI / 180.0f)
#define RAD_TO_DEG      (180.0f / M_PI)


float wrap_2PI(const float radian)
{
    float res = fmod(radian, M_2PI);
    if (res < 0) {
        res += M_2PI;
    }
    return res;
}

float wrap_PI(const float radian)
{
    float res = wrap_2PI(radian);
    if (res > M_PI) {
        res -= M_2PI;
    }
    return res;
}


template <typename T>
T constrain_value_line(const T amt, const T low, const T high)
{
    // the check for NaN as a float prevents propagation of floating point
    // errors through any function that uses constrain_value(). The normal
    // float semantics already handle -Inf and +Inf
    if (std::isnan(amt)) {
        return (low + high) / 2;
    }

    if (amt < low) {
        return low;
    }

    if (amt > high) {
        return high;
    }

    return amt;
}

#define constrain_float(amt, low, high) constrain_value_line(float(amt), float(low), float(high))


template <typename T>
T constrain_value(const T amt, const T low, const T high)
{
    // the check for NaN as a float prevents propagation of floating point
    // errors through any function that uses constrain_value(). The normal
    // float semantics already handle -Inf and +Inf
    if (std::is_floating_point<T>::value) {
        if (std::isnan(amt)) {
            return (low + high) / 2;
        }
    }

    if (amt < low) {
        return low;
    }

    if (amt > high) {
        return high;
    }

    return amt;
}

inline int16_t constrain_int16(const int16_t amt, const int16_t low, const int16_t high)
{
    return constrain_value(amt, low, high);
}



// degrees -> radians
static inline constexpr double radians(double deg)
{
    return deg * DEG_TO_RAD;
}

// radians -> degrees
static inline constexpr float degrees(float rad)
{
    return rad * RAD_TO_DEG;
}

static void sleepMs(uint32_t timeMs)
{
	usleep(timeMs * 1000);
}



/*
  replacement for std::swap() needed for STM32
 */
static void swap_float(float &f1, float &f2)
{
    float tmp = f1;
    f1 = f2;
    f2 = tmp;
}

/*
 * linear interpolation based on a variable in a range
 */
float linear_interpolate(float low_output, float high_output,
                         float var_value,
                         float var_low, float var_high)
{
    if (var_low > var_high) {
        // support either polarity
        swap_float(var_low, var_high);
        swap_float(low_output, high_output);
    }
    if (var_value <= var_low) {
        return low_output;
    }
    if (var_value >= var_high) {
        return high_output;
    }
    float p = (var_value - var_low) / (var_high - var_low);
    return low_output + p * (high_output - low_output);
}

/*
 * @brief: Check whether a double is greater than zero
 */
inline bool is_positive(const double fVal1) {
    return (fVal1 >= static_cast<double>(FLT_EPSILON));
}

/*
 * @brief: Check whether a double is less than zero
 */
inline bool is_negative(const double fVal1) {
    return (fVal1 <= static_cast<double>((-1.0 * FLT_EPSILON)));
}


static int64_t getTimeMillis()
{
	int64_t timeMs = std::chrono::duration_cast<std::chrono::milliseconds>(std::chrono::system_clock::now().time_since_epoch()).count();

	return timeMs;
}


// hardware lookup table indexed by HardwareModel enum values
const MountSiyi::HWInfo MountSiyi::hardware_lookup_table[] {
        {{'0','0'}, "Unknown"},
        {{'7','5'}, "A2"},
        {{'7','3'}, "A8"},
        {{'6','B'}, "ZR10"},
        {{'7','8'}, "ZR30"},
        {{'8','2'}, "ZT6"},
        {{'7','A'}, "ZT30"},
};


MountSiyi::MountSiyi():
	_initialised(false),
	m_interfaceType(UNKNOW),
	m_tcpSockfd(-1),
	m_udpSockfd(-1),
	m_siyiVideoCallbackFunction(NULL),
	m_siyiAttitudeCallbackFunction(NULL),
	m_rtspHealthy(false)
{
	m_sendBuffer.m_buffer = NULL;
}
MountSiyi::~MountSiyi()
{
	_initialised = false;

	if(m_tcpSockfd > 0)
	{
		close(m_tcpSockfd);
		m_tcpSockfd = -1;
	}

	if(m_udpSockfd > 0)
	{
		close(m_udpSockfd);
		m_udpSockfd = -1;
	}

	freeCircularBuf(&m_sendBuffer);
}

bool MountSiyi::init(const char *sockType, const char *ip)
{
	bool ret;

	if(initCircularBuf(&m_sendBuffer, CIRCULAR_BUFFER_SIZE) < 0)
	{
		printf("send initCircularBuf failed\n");
		return false;
	}

	if(strcmp(sockType, "tcp") == 0)
	{
		printf("sock type is tcp.\n");
		ret = tcpClient(ip, MOUNT_SIYI_TCP_PORT);
		if(ret)
		{
			_initialised = true;
			m_interfaceType = SIYI_TCP;
			return true;
		}
	}
	else if(strcmp(sockType, "udp") == 0)
	{
		printf("sock type is udp.\n");
		ret = udpClient(ip, MOUNT_SIYI_TCP_PORT);
		if(ret)
		{
			_initialised = true;
			m_interfaceType = SIYI_UDP;
			return true;
		}
	}
	else
	{
		printf("sock type is error:%s\n", sockType);
	}

	return false;
}

// 100hz
void MountSiyi::update()
{
	while(_initialised)
	{	
		// 100hz
		sleepMs(10);

		// request firmware version during startup at 1hz
		// during regular operation request configuration at 1hz
		uint32_t now_ms = getTimeMillis();
		if ((now_ms - _last_send_ms) >= 1000) {
			_last_send_ms = now_ms;
			if (!_got_hardware_id) {
				request_hardware_id();
				continue;
			} else if (!_fw_version.received) {
				request_firmware_version();
				continue;
			} else {
				request_configuration();
			}
		}

		// request attitude at regular intervals
    	if ((now_ms - _last_req_current_angle_rad_ms) >= 50) {
        	request_gimbal_attitude();
        	_last_req_current_angle_rad_ms = now_ms;
    	}

		// request rangefinder distance from ZT30 at 10hz
    	if ((_hardware_model == HardwareModel::ZT30) && (now_ms - _last_rangefinder_req_ms > 100)) {
        	request_rangefinder_distance();
        	_last_rangefinder_req_ms = now_ms;
    	}

		// run zoom control
    	update_zoom_control();
	}
}


bool MountSiyi::healthy()
{
    // unhealthy until gimbal has been found and replied with firmware version info
    if (!_initialised || !_fw_version.received) {
        return false;
    }

    // unhealthy if attitude information NOT received recently
    const uint32_t now_ms = getTimeMillis();
    if (now_ms - _last_current_angle_rad_ms > MOUNT_SIYI_TIMEOUT_MS) {
        return false;
    }

    // if we get this far return healthy
    return true;
}


bool MountSiyi::registerSiyiVideoCallback(siyiVideoCallback function)
{
	if(m_siyiVideoCallbackFunction != NULL)
	{
		return false;
	}

	m_siyiVideoCallbackFunction = function;
	return true;
}

bool MountSiyi::registerSiyiAttitudeCallback(siyiAttitudeCallback function)
{
	if(m_siyiAttitudeCallbackFunction != NULL)
	{
		return false;
	}

	m_siyiAttitudeCallbackFunction = function;
	return true;
}


bool MountSiyi::take_picture()
{
	return send_1byte_packet(SiyiCommandId::PHOTO, (uint8_t)PhotoFunction::TAKE_PICTURE);
}

bool MountSiyi::record_video(bool start_recording)

{
    // exit immediately if not initialised to reduce mismatch
    // between internal and actual state of recording
    if (!_initialised) {
        return false;
    }

    bool success = true;
    bool send_toggle = false;
    if (start_recording) {
        switch (_config_info.record_status) {
            case RecordingStatus::ON:
                // already recording...
                break;
            // assume that DATA_LOSS is the same as OFF
            case RecordingStatus::DATA_LOSS:
            case RecordingStatus::OFF:
                send_toggle = true;
                break;
            case RecordingStatus::NO_CARD:
                printf("Siyi: can't start recording: No Card\n");
                success = false;
                break;
        }
    } else {
        switch (_config_info.record_status) {
            case RecordingStatus::ON:
                send_toggle = true;
                break;
            // assume that DATA_LOSS is the same as OFF
            case RecordingStatus::DATA_LOSS:
            case RecordingStatus::OFF:
            case RecordingStatus::NO_CARD:
                // already off...
                break;
        }
    }

    if (send_toggle) {
        success = send_1byte_packet(SiyiCommandId::PHOTO, (uint8_t)PhotoFunction::RECORD_VIDEO_TOGGLE);
    }

    // request recording state update from gimbal
    request_configuration();

    return success;
}


bool MountSiyi::set_zoom(ZoomType zoom_type, float zoom_value)
{
    switch (zoom_type) {
    case ZoomType::RATE:
        if (send_zoom_rate(zoom_value)) {
            _zoom_type = zoom_type;
            _zoom_rate_target = zoom_value;
            return true;
        }
        return false;
    case ZoomType::PCT: {
        // absolute zoom
        float zoom_mult_max = get_zoom_mult_max();
        if (is_positive(zoom_mult_max)) {
            // convert zoom percentage (0~100) to target zoom multiple (e.g. 0~6x or 0~30x)
            const float zoom_mult = linear_interpolate(1, zoom_mult_max, zoom_value, 0, 100);
            if (send_zoom_mult(zoom_mult)) {
                _zoom_type = zoom_type;
                return true;
            }
            return false;
        }
        return false;
    }
    }

    // unsupported zoom type
    return false;
}


SetFocusResult MountSiyi::set_focus(FocusType focus_type, float focus_value)
{
    switch (focus_type) {
    case FocusType::RATE: {
        uint8_t focus_step = 0;
        if (focus_value > 0) {
            focus_step = 1;
        } else if (focus_value < 0) {
            // Siyi API specifies -1 should be sent as 255
            focus_step = UINT8_MAX;
        }
        if (!send_1byte_packet(SiyiCommandId::MANUAL_FOCUS, (uint8_t)focus_step)) {
            return SetFocusResult::FAILED;
        }
        return SetFocusResult::ACCEPTED;
    }
    case FocusType::PCT:
        // not supported
        return SetFocusResult::INVALID_PARAMETERS;
    case FocusType::AUTO:
        if (!send_1byte_packet(SiyiCommandId::AUTO_FOCUS, 1)) {
            return SetFocusResult::FAILED;
        }
        return SetFocusResult::ACCEPTED;
    }

    // unsupported focus type
    return SetFocusResult::INVALID_PARAMETERS;
}

bool MountSiyi::set_lens(uint8_t lens)
{
    // only supported on ZT30.  sanity check lens values
    if ((_hardware_model != HardwareModel::ZT30) || (lens > 8)) {
        return false;
    }

    // maps lens to siyi camera image type so that lens of 0, 1, 2 are more useful
    CameraImageType cam_image_type = CameraImageType::MAIN_ZOOM_SUB_THERMAL;
    switch (lens) {
        case 0:
            cam_image_type = CameraImageType::MAIN_ZOOM_SUB_THERMAL; // 3
            break;
        case 1:
            cam_image_type = CameraImageType::MAIN_WIDEANGLE_SUB_THERMAL; // 5
            break;
        case 2:
            cam_image_type = CameraImageType::MAIN_THERMAL_SUB_ZOOM; // 7
            break;
        case 3:
            cam_image_type = CameraImageType::MAIN_PIP_ZOOM_THERMAL_SUB_WIDEANGLE; // 0
            break;
        case 4:
            cam_image_type = CameraImageType::MAIN_PIP_WIDEANGLE_THERMAL_SUB_ZOOM; // 1
            break;
        case 5:
            cam_image_type = CameraImageType::MAIN_PIP_ZOOM_WIDEANGLE_SUB_THERMAL; // 2
            break;
        case 6:
            cam_image_type = CameraImageType::MAIN_ZOOM_SUB_WIDEANGLE; // 4
            break;
        case 7:
            cam_image_type = CameraImageType::MAIN_WIDEANGLE_SUB_ZOOM; // 6
            break;
        case 8:
            cam_image_type = CameraImageType::MAIN_THERMAL_SUB_WIDEANGLE; // 8
            break;
    }

    // send desired image type to camera
    return send_1byte_packet(SiyiCommandId::SET_CAMERA_IMAGE_TYPE, (uint8_t)cam_image_type);
}


bool MountSiyi::set_camera_source(uint8_t primary_source, uint8_t secondary_source)
{
    // only supported on ZT30.  sanity check lens values
    if (_hardware_model != HardwareModel::ZT30) {
        return false;
    }

    // maps primary and secondary source to siyi camera image type
    CameraImageType cam_image_type;
    switch (primary_source) {
    case 0: // Default (RGB)
    case 1: // RGB
        switch (secondary_source) {
        case 0: // RGB + Default (None)
            cam_image_type = CameraImageType::MAIN_ZOOM_SUB_THERMAL;                // 3
            break;
        case 2: // PIP RGB+IR
            cam_image_type = CameraImageType::MAIN_PIP_ZOOM_THERMAL_SUB_WIDEANGLE;  // 0
            break;
        case 4: // PIP RGB+RGB_WIDEANGLE
            cam_image_type = CameraImageType::MAIN_PIP_ZOOM_WIDEANGLE_SUB_THERMAL;  // 2
            break;
        default:
            return false;
        }
        break;
    case 2: // IR
        switch (secondary_source) {
        case 0: // IR + Default (None)
            cam_image_type = CameraImageType::MAIN_THERMAL_SUB_ZOOM;                // 7
            break;
        default:
            return false;
        }
        break;
    case 4: // RGB_WIDEANGLE
        switch (secondary_source) {
        case 0: // RGB_WIDEANGLE + Default (None)
            cam_image_type = CameraImageType::MAIN_WIDEANGLE_SUB_THERMAL;           // 5
            break;
        case 2: // PIP RGB_WIDEANGLE+IR
            cam_image_type = CameraImageType::MAIN_PIP_WIDEANGLE_THERMAL_SUB_ZOOM;  // 1
            break;
        default:
            return false;
        }
        break;
    default:
        return false;
    }

    // send desired image type to camera
    return send_1byte_packet(SiyiCommandId::SET_CAMERA_IMAGE_TYPE, (uint8_t)cam_image_type);
}

bool MountSiyi::get_rangefinder_distance(float& distance_m)
{
    // only supported on ZT30
    if (_hardware_model != HardwareModel::ZT30) {
        return false;
    }

    // unhealthy if distance not received recently
    const uint32_t now_ms = getTimeMillis();
    if (now_ms - _last_rangefinder_dist_ms > MOUNT_SIYI_TIMEOUT_MS) {
        return false;
    }

    distance_m = _rangefinder_dist_m;
    return true;
}


bool MountSiyi::startRtspRecv(const char *rtsp_url)
{
	cv::VideoCapture capture(rtsp_url);

	if (!capture.isOpened())
	{
		printf("siyi video file open error.rtsp url:%s\n", rtsp_url);
		return false;
	}

	int width = capture.get(cv::CAP_PROP_FRAME_WIDTH);
	int height = capture.get(cv::CAP_PROP_FRAME_HEIGHT);
	int count = capture.get(cv::CAP_PROP_FRAME_COUNT);
	double fps = capture.get(cv::CAP_PROP_FPS);
	cout << "resolution: " << width << " * " << height << endl;
	cout << "count: " << count << endl;
	cout << "fps: " << fps << endl;
	double interval = 1.0 / fps;
	cout << "interval: " << interval << endl;

	cv::Mat im(cv::Size(width, height), CV_16UC4);
	int testCout=0;

	while(1)
	{
		std::chrono::steady_clock::time_point t1 = std::chrono::steady_clock::now();
		capture >> im;

		if (nullptr == im.ptr() || 0 == im.rows || 0 == im.cols)
		{
			m_rtspHealthy = false;
			//rtsp restart.
			capture.release();
			capture.open(rtsp_url);
			sleepMs(100);
			continue;
		}

		//callback
		if(m_siyiVideoCallbackFunction != NULL)
		{
			(*m_siyiVideoCallbackFunction)(im);
		}
#if 0
		char testImageName[30];
		memset(testImageName, 0, sizeof(testImageName) / sizeof(testImageName[0]));
		testCout++;
		printf("testimagecount :%d\n", testCout);
		sprintf(testImageName,"./testImage/%d.jpg",testCout);
		imwrite(testImageName,im);
#endif

		std::chrono::steady_clock::time_point t2 = std::chrono::steady_clock::now();

		double ttrack = std::chrono::duration_cast<std::chrono::duration<double>>(t2 - t1).count();
		//cout << "ttrack:" << ttrack * 1e3  << endl;

		// Wait to load the next frame
		int delay = (interval - ttrack) * 1e3;
		delay = delay <= 0 ? 1 : delay;

#if 1
		sleepMs(delay);
#else
		imshow("video", im);
		waitKey(delay);
#endif
		m_rtspHealthy = true;
	}
}


bool MountSiyi::getRtspHealthy()
{
	return m_rtspHealthy;
}



void MountSiyi::read_incoming_packets(uint8_t *buf, uint16_t nbytes)
{
    if (nbytes <= 0  || buf == NULL) {
        return;
    }

    // flag to allow cases below to reset parser state
    bool reset_parser = false;

    // process bytes received
    for (int16_t i = 0; i < nbytes; i++) {
        uint8_t b;
        b = buf[i];

        _msg_buff[_msg_buff_len++] = b;

        // protect against overly long messages
        if (_msg_buff_len >= MOUNT_SIYI_PACKETLEN_MAX) {
            reset_parser = true;
        }

        // process byte depending upon current state
        switch (_parsed_msg.state) {

        case ParseState::WAITING_FOR_HEADER_LOW:
            if (b == MOUNT_SIYI_HEADER1) {
                _parsed_msg.state = ParseState::WAITING_FOR_HEADER_HIGH;
            } else {
                reset_parser = true;
            }
            break;

        case ParseState::WAITING_FOR_HEADER_HIGH:
            if (b == MOUNT_SIYI_HEADER2) {
                _parsed_msg.state = ParseState::WAITING_FOR_CTRL;
            } else {
                reset_parser = true;
            }
            break;

        case ParseState::WAITING_FOR_CTRL:
            _parsed_msg.state = ParseState::WAITING_FOR_DATALEN_LOW;
            break;

        case ParseState::WAITING_FOR_DATALEN_LOW:
            _parsed_msg.data_len = b;
            _parsed_msg.state = ParseState::WAITING_FOR_DATALEN_HIGH;
            break;

        case ParseState::WAITING_FOR_DATALEN_HIGH:
            _parsed_msg.data_len |= ((uint16_t)b << 8);
            // sanity check data length
            if (_parsed_msg.data_len <= MOUNT_SIYI_DATALEN_MAX) {
                _parsed_msg.state = ParseState::WAITING_FOR_SEQ_LOW;
            } else {
                reset_parser = true;
                //printf("data len too large:%u (>%u)", (unsigned)_parsed_msg.data_len, (unsigned)AP_MOUNT_SIYI_DATALEN_MAX);
            }
            break;

        case ParseState::WAITING_FOR_SEQ_LOW:
            _parsed_msg.state = ParseState::WAITING_FOR_SEQ_HIGH;
            break;

        case ParseState::WAITING_FOR_SEQ_HIGH:
            _parsed_msg.state = ParseState::WAITING_FOR_CMDID;
            break;

        case ParseState::WAITING_FOR_CMDID:
            _parsed_msg.command_id = b;
            _parsed_msg.data_bytes_received = 0;
            if (_parsed_msg.data_len > 0) {
                _parsed_msg.state = ParseState::WAITING_FOR_DATA;
            } else {
                _parsed_msg.state = ParseState::WAITING_FOR_CRC_LOW;
            }
            break;

        case ParseState::WAITING_FOR_DATA:
            _parsed_msg.data_bytes_received++;
            if (_parsed_msg.data_bytes_received >= _parsed_msg.data_len) {
                _parsed_msg.state = ParseState::WAITING_FOR_CRC_LOW;
            }
            break;

        case ParseState::WAITING_FOR_CRC_LOW:
            _parsed_msg.crc16 = b;
            _parsed_msg.state = ParseState::WAITING_FOR_CRC_HIGH;
            break;

        case ParseState::WAITING_FOR_CRC_HIGH:
            _parsed_msg.crc16 |= ((uint16_t)b << 8);

            // check crc
            const uint16_t expected_crc = crc16_ccitt(_msg_buff, _msg_buff_len-2, 0);
            if (expected_crc == _parsed_msg.crc16) {
                // successfully received a message, do something with it
                process_packet();
#if MOUNT_SIYI_DEBUG
            } else {
                printf("crc expected:%x got:%x\n", (unsigned)expected_crc, (unsigned)_parsed_msg.crc16);
#endif
            }
            reset_parser = true;
            break;
        }

        // handle reset of parser
        if (reset_parser) {
            _parsed_msg.state = ParseState::WAITING_FOR_HEADER_LOW;
            _msg_buff_len = 0;
            reset_parser = false;
        }
    }
}


void MountSiyi::process_packet()
{
#if MOUNT_SIYI_DEBUG
    // flag to warn of unexpected data buffer length
    bool unexpected_len = false;
#endif

    // process packet depending upon command id
    switch ((SiyiCommandId)_parsed_msg.command_id) {

	case SiyiCommandId::HEARTBEAT:	{
			printf("heartBeat\n");
			break;
		}

    case SiyiCommandId::ACQUIRE_FIRMWARE_VERSION: {
        if (_parsed_msg.data_bytes_received != 12 &&    // ZR10 firmware version reply is 12bytes
            _parsed_msg.data_bytes_received != 8) {     // A8 firmware version reply is 8 bytes
#if MOUNT_SIYI_DEBUG
            unexpected_len = true;
#endif
            break;
        }

        // consume and display camera firmware version
        _fw_version = {};
        _fw_version.camera.major = _msg_buff[_msg_buff_data_start+2];  // firmware major version
        _fw_version.camera.minor = _msg_buff[_msg_buff_data_start+1];  // firmware minor version
        _fw_version.camera.patch = _msg_buff[_msg_buff_data_start+0];  // firmware revision (aka patch)

        _fw_version.gimbal.major = _msg_buff[_msg_buff_data_start+6];  // firmware major version
        _fw_version.gimbal.minor = _msg_buff[_msg_buff_data_start+5];  // firmware minor version
        _fw_version.gimbal.patch = _msg_buff[_msg_buff_data_start+4];  // firmware revision (aka patch)

        // camera firmware version may be all zero soon after startup.  giveup and try again later
        if (_fw_version.camera.major == 0 && _fw_version.camera.minor == 0 && _fw_version.camera.patch == 0) {
            break;
        }

        _fw_version.received = true;


        // display zoom firmware version for those that have it
        if (_parsed_msg.data_bytes_received >= 12) {
            _fw_version.zoom.major = _msg_buff[_msg_buff_data_start+10];
            _fw_version.zoom.minor = _msg_buff[_msg_buff_data_start+ 9];
            _fw_version.zoom.patch = _msg_buff[_msg_buff_data_start+ 8];
        }

        // report to the user if gimbal firmware is not up-to-date
        check_firmware_version();

        break;
    }

    case SiyiCommandId::HARDWARE_ID: {
        // lookup first two digits of hardware id
        const uint8_t hwid0 = _msg_buff[_msg_buff_data_start];
        const uint8_t hwid1 = _msg_buff[_msg_buff_data_start+1];
        for (uint8_t i=1; i<ARRAY_SIZE(hardware_lookup_table); i++) {
            if (hwid0 == hardware_lookup_table[i].hwid[0] && hwid1 == hardware_lookup_table[i].hwid[1]) {
               _hardware_model = (HardwareModel)i;
            }
        }
        _got_hardware_id = true;
        break;
    }

    case SiyiCommandId::AUTO_FOCUS:
#if MOUNT_SIYI_DEBUG
        if (_parsed_msg.data_bytes_received != 1) {
            unexpected_len = true;
            break;
        }
        printf("AutoFocus:%u\n", (unsigned)_msg_buff[_msg_buff_data_start]);
#endif
        break;

    case SiyiCommandId::MANUAL_ZOOM_AND_AUTO_FOCUS: {
        if (_parsed_msg.data_bytes_received != 2) {
#if MOUNT_SIYI_DEBUG
            unexpected_len = true;
#endif
            break;
        }
        _zoom_mult = UINT16_VALUE(_msg_buff[_msg_buff_data_start+1], _msg_buff[_msg_buff_data_start]) * 0.1;
        printf("ZoomMult:%4.1f\n", (double)_zoom_mult);
        break;
    }

    case SiyiCommandId::MANUAL_FOCUS:
#if MOUNT_SIYI_DEBUG
        if (_parsed_msg.data_bytes_received != 1) {
            unexpected_len = true;
            break;
        }
        printf("ManualFocus:%u\n", (unsigned)_msg_buff[_msg_buff_data_start]);
#endif
        break;

    case SiyiCommandId::GIMBAL_ROTATION:
#if MOUNT_SIYI_DEBUG
        if (_parsed_msg.data_bytes_received != 1) {
            unexpected_len = true;
            break;
        }
        printf("GimbRot:%u\n", (unsigned)_msg_buff[_msg_buff_data_start]);
#endif
        break;

    case SiyiCommandId::CENTER:
#if MOUNT_SIYI_DEBUG
        if (_parsed_msg.data_bytes_received != 1) {
            unexpected_len = true;
            break;
        }
        printf("Center:%u\n", (unsigned)_msg_buff[_msg_buff_data_start]);
#endif
        break;

    case SiyiCommandId::ACQUIRE_GIMBAL_CONFIG_INFO: {
        const RecordingStatus prev_record_status = _config_info.record_status;

        // Update Gimbal Config Information
        size_t config_sz = MIN(_parsed_msg.data_bytes_received, sizeof(_config_info));
        memcpy(&_config_info, &_msg_buff[_msg_buff_data_start], config_sz);

        // Alert user if recording status changed
        if (prev_record_status != _config_info.record_status) {
            switch (_config_info.record_status) {
                case RecordingStatus::OFF:
					printf("record off\n");
                    break;
                case RecordingStatus::ON:
					printf("record on\n");
                    break;
                case RecordingStatus::NO_CARD:
					printf("NO CARD\n");
                    break;
                case RecordingStatus::DATA_LOSS:
					printf("DATA LOSS\n");
                    break;
            }
        }

        printf(
            "GimConf hdr:%u rec:%u foll:%u mntdir:%u vid:%u\n",
            (uint8_t)_config_info.hdr_status,
            (uint8_t)_config_info.record_status,
            (uint8_t)_config_info.motion_mode,
            (uint8_t)_config_info.mounting_dir,
            (uint8_t)_config_info.video_mode
        );
        break;
    }

    case SiyiCommandId::FUNCTION_FEEDBACK_INFO: {
        if (_parsed_msg.data_bytes_received != 1) {
#if MOUNT_SIYI_DEBUG
            unexpected_len = true;
#endif
            break;
        }
        const uint8_t func_feedback_info = _msg_buff[_msg_buff_data_start];
        const char* err_prefix = "Mount: Siyi";
        (void)err_prefix;  // in case !HAL_GCS_ENABLED
        switch ((FunctionFeedbackInfo)func_feedback_info) {
        case FunctionFeedbackInfo::SUCCESS:
            printf("FnFeedB success\n");
            break;
        case FunctionFeedbackInfo::FAILED_TO_TAKE_PHOTO:
            printf("%s failed to take picture\n", err_prefix);
            break;
        case FunctionFeedbackInfo::HDR_ON:
            printf("HDR on\n");
            break;
        case FunctionFeedbackInfo::HDR_OFF:
            printf("HDR off\n");
            break;
        case FunctionFeedbackInfo::FAILED_TO_RECORD_VIDEO:
            printf("%s failed to record video\n", err_prefix);
            break;
        default:
            printf("FnFeedB unexpected val:%u\n", (unsigned)func_feedback_info);
        }
        break;
    }

    case SiyiCommandId::PHOTO:
        // no ack should ever be sent by the gimbal
        break;

    case SiyiCommandId::ACQUIRE_GIMBAL_ATTITUDE: {
        if (_parsed_msg.data_bytes_received != 12) {
#if MOUNT_SIYI_DEBUG
            unexpected_len = true;
#endif
            break;
        }
        _last_current_angle_rad_ms = getTimeMillis();
        _current_angle_rad.z = -radians((int16_t)UINT16_VALUE(_msg_buff[_msg_buff_data_start+1], _msg_buff[_msg_buff_data_start]) * 0.1);   // yaw angle
        _current_angle_rad.y = radians((int16_t)UINT16_VALUE(_msg_buff[_msg_buff_data_start+3], _msg_buff[_msg_buff_data_start+2]) * 0.1);  // pitch angle
        _current_angle_rad.x = radians((int16_t)UINT16_VALUE(_msg_buff[_msg_buff_data_start+5], _msg_buff[_msg_buff_data_start+4]) * 0.1);  // roll angle
        _current_rates_rads.z = -radians((int16_t)UINT16_VALUE(_msg_buff[_msg_buff_data_start+7], _msg_buff[_msg_buff_data_start+6]) * 0.1);   // yaw rate
        _current_rates_rads.y = radians((int16_t)UINT16_VALUE(_msg_buff[_msg_buff_data_start+9], _msg_buff[_msg_buff_data_start+8]) * 0.1);   // pitch rate
        _current_rates_rads.x = radians((int16_t)UINT16_VALUE(_msg_buff[_msg_buff_data_start+11], _msg_buff[_msg_buff_data_start+10]) * 0.1);  // roll rate

		if(m_siyiAttitudeCallbackFunction != NULL)
		{
			(*m_siyiAttitudeCallbackFunction)(degrees(_current_angle_rad.x), degrees(_current_angle_rad.y), degrees(_current_angle_rad.z));
		}
        break;
    }

    case SiyiCommandId::READ_RANGEFINDER: {
        _rangefinder_dist_m = UINT16_VALUE(_msg_buff[_msg_buff_data_start+1], _msg_buff[_msg_buff_data_start]);
        _last_rangefinder_dist_ms = getTimeMillis();
        break;
    }

    default:
        printf("Unhandled CmdId:%u\n", (unsigned)_parsed_msg.command_id);
        break;
    }

#if MOUNT_SIYI_DEBUG
    // handle unexpected data buffer length
    if (unexpected_len) {
        printf("CmdId:%u unexpected len:%u\n", (unsigned)_parsed_msg.command_id, (unsigned)_parsed_msg.data_bytes_received);
    }
#endif
}


bool MountSiyi::send_packet(SiyiCommandId cmd_id, const uint8_t* databuff, uint8_t databuff_len)
{
    if (!_initialised) {
        return false;
    }
    // calculate and sanity check packet size
    const uint16_t packet_size = MOUNT_SIYI_PACKETLEN_MIN + databuff_len;
    if (packet_size > MOUNT_SIYI_PACKETLEN_MAX) {
        printf("send_packet data buff too large\n");
        return false;
    }

    // buffer for holding outgoing packet
    uint8_t send_buff[packet_size];
    uint8_t send_buff_ofs = 0;

    // packet header
    send_buff[send_buff_ofs++] = MOUNT_SIYI_HEADER1;
    send_buff[send_buff_ofs++] = MOUNT_SIYI_HEADER2;

    // CTRL.  Always request ACK
    send_buff[send_buff_ofs++] = 1;

    // Data_len.  protocol supports uint16_t but messages are never longer than 22 bytes
    send_buff[send_buff_ofs++] = databuff_len;
    send_buff[send_buff_ofs++] = 0;

    // SEQ (sequence)
    send_buff[send_buff_ofs++] = LOWBYTE(_last_seq);
    send_buff[send_buff_ofs++] = HIGHBYTE(_last_seq++);

    // CMD_ID
    send_buff[send_buff_ofs++] = (uint8_t)cmd_id;

    // DATA
    if (databuff_len != 0) {
        memcpy(&send_buff[send_buff_ofs], databuff, databuff_len);
        send_buff_ofs += databuff_len;
    }

    // CRC16
    const uint16_t crc = crc16_ccitt(send_buff, send_buff_ofs, 0);
    send_buff[send_buff_ofs++] = LOWBYTE(crc);
    send_buff[send_buff_ofs++] = HIGHBYTE(crc);

    // send packet
    writeCircularBuf(&m_sendBuffer, send_buff, send_buff_ofs);

    return true;
}


bool MountSiyi::send_1byte_packet(SiyiCommandId cmd_id, uint8_t data_byte)
{
    return send_packet(cmd_id, &data_byte, 1);
}

void MountSiyi::rotate_gimbal(int8_t pitch_scalar, int8_t yaw_scalar, bool yaw_is_ef)
{
    // send lock/follow value
    const GimbalMotionMode mode = yaw_is_ef ? GimbalMotionMode::LOCK : GimbalMotionMode::FOLLOW;
    if (!set_motion_mode(mode)) {
        // couldn't set mode, so don't send rotation
        return;
    }

    const uint8_t yaw_and_pitch_rates[] {(uint8_t)yaw_scalar, (uint8_t)pitch_scalar};
    send_packet(SiyiCommandId::GIMBAL_ROTATION, yaw_and_pitch_rates, ARRAY_SIZE(yaw_and_pitch_rates));
}


void MountSiyi::center_gimbal()
{
    send_1byte_packet(SiyiCommandId::CENTER, 1);
}

bool MountSiyi::set_motion_mode(const GimbalMotionMode mode, const bool force)
{
    if (!force && (mode == _config_info.motion_mode)) {
        // we're already in the right mode...
        return true;
    }

    PhotoFunction data = PhotoFunction::LOCK_MODE;
    switch (mode) {
        case GimbalMotionMode::LOCK:   data = PhotoFunction::LOCK_MODE; break;
        case GimbalMotionMode::FOLLOW: data = PhotoFunction::FOLLOW_MODE; break;
        case GimbalMotionMode::FPV:    data = PhotoFunction::FPV_MODE; break;
    }
    bool sent = send_1byte_packet(SiyiCommandId::PHOTO, (uint8_t)data);
    if (sent) {
        // assume the mode is set correctly until told otherwise
        _config_info.motion_mode = mode;
        request_configuration();
    }
    return sent;
}


void MountSiyi::send_target_rates(float pitch_rads, float yaw_rads, bool yaw_is_ef)
{
    const float pitch_rate_scalar = constrain_float(100.0 * pitch_rads / MOUNT_SIYI_RATE_MAX_RADS, -100, 100);
    const float yaw_rate_scalar = constrain_float(100.0 * yaw_rads / MOUNT_SIYI_RATE_MAX_RADS, -100, 100);
    rotate_gimbal(pitch_rate_scalar, yaw_rate_scalar, yaw_is_ef);
}


void MountSiyi::send_target_angles(float pitch_rad, float yaw_rad, bool yaw_is_ef, float flight_yaw)
{
    // stop gimbal if no recent actual angles
    uint32_t now_ms = getTimeMillis();
    if (now_ms - _last_current_angle_rad_ms >= 200) {
        rotate_gimbal(0, 0, false);
        return;
    }

    // if gimbal mounting direction is 2 i.e. upside down, then transform the angles
    Vector3f current_angle_transformed = _current_angle_rad;
    if (_config_info.mounting_dir == GimbalMountingDirection::UPSIDE_DOWN) {
        current_angle_transformed.y = -wrap_PI(_current_angle_rad.y + M_PI);
        current_angle_transformed.z = -_current_angle_rad.z;
    }

    // use simple P controller to convert pitch angle error (in radians) to a target rate scalar (-100 to +100)
    const float pitch_err_rad = (pitch_rad - current_angle_transformed.y);
    const float pitch_rate_scalar = constrain_float(100.0 * pitch_err_rad * MOUNT_SIYI_PITCH_P / MOUNT_SIYI_RATE_MAX_RADS, -100, 100);

    // convert yaw angle to body-frame
    float yaw_bf_rad = yaw_is_ef ? wrap_PI(yaw_rad - flight_yaw) : yaw_rad;

    // enforce body-frame yaw angle limits.  If beyond limits always use body-frame control
    const float yaw_bf_min = radians(MOUNT_YAW_ANGLE_MIN);
    const float yaw_bf_max = radians(MOUNT_YAW_ANGLE_MAX);
    if (yaw_bf_rad < yaw_bf_min || yaw_bf_rad > yaw_bf_max) {
        yaw_bf_rad = constrain_float(yaw_bf_rad, yaw_bf_min, yaw_bf_max);
        yaw_is_ef = false;
    }

    // use simple P controller to convert yaw angle error to a target rate scalar (-100 to +100)
    const float yaw_err_rad = (yaw_bf_rad - current_angle_transformed.z);
    const float yaw_rate_scalar = constrain_float(100.0 * yaw_err_rad * MOUNT_SIYI_YAW_P / MOUNT_SIYI_RATE_MAX_RADS, -100, 100);

    // rotate gimbal.  pitch_rate and yaw_rate are scalars in the range -100 ~ +100
    rotate_gimbal(pitch_rate_scalar, yaw_rate_scalar, yaw_is_ef);
}


bool MountSiyi::send_zoom_rate(float zoom_value)
{
    uint8_t zoom_step = 0;
    if (zoom_value > 0) {
        // zoom in
        zoom_step = 1;
    }
    if (zoom_value < 0) {
        // zoom out. Siyi API specifies -1 should be sent as 255
        zoom_step = UINT8_MAX;
    }
    return send_1byte_packet(SiyiCommandId::MANUAL_ZOOM_AND_AUTO_FOCUS, zoom_step);
}


bool MountSiyi::send_zoom_mult(float zoom_mult)
{
    // separate zoom_mult into integral and fractional parts
    double intpart;
    uint8_t fracpart = (uint8_t)constrain_int16(modf(zoom_mult, &intpart) * 10, 0, UINT8_MAX);

    // create and send 2 byte array
    const uint8_t zoom_mult_data[] {(uint8_t)(intpart), fracpart};
    return send_packet(SiyiCommandId::ABSOLUTE_ZOOM, zoom_mult_data, ARRAY_SIZE(zoom_mult_data));
}

float MountSiyi::get_zoom_mult_max()
{
    switch (_hardware_model) {
    case HardwareModel::UNKNOWN:
        return 0;
    case HardwareModel::A2:
    case HardwareModel::A8:
    case HardwareModel::ZT6:
        // a8, zt6 have 6x digital zoom
        return 6;
    case HardwareModel::ZR10:
    case HardwareModel::ZR30:
    case HardwareModel::ZT30:
        // 30x hybrid zoom (optical + digital)
        return 30;
    }
    return 0;
}

void MountSiyi::update_zoom_control()
{
    if (_zoom_type == ZoomType::RATE) {
        // limit updates to 1hz
        const uint32_t now_ms = getTimeMillis();
        if (now_ms - _last_zoom_control_ms < 1000) {
            return;
        }
        _last_zoom_control_ms = now_ms;

        // only send zoom rate target if it's non-zero because if zero it has already been sent
        // and sending zero rate also triggers autofocus
        if (!iszero(_zoom_rate_target)) {
            send_zoom_rate(_zoom_rate_target);
        }
    }
}


const char* MountSiyi::get_model_name()
{
    uint8_t model_idx = (uint8_t)_hardware_model;
    if (model_idx < ARRAY_SIZE(hardware_lookup_table)) {
        return hardware_lookup_table[model_idx].model_name;
    }
    return hardware_lookup_table[0].model_name;
}

void MountSiyi::check_firmware_version()
{
    if (!_fw_version.received) {
        printf("Can't check firmware if we haven't received it...\n");
        return;
    }

    if (!_got_hardware_id) {
        printf("Can't check firmware without Hardware ID!\n");
        return;
    }

    FirmwareVersion minimum_ver {};
    switch (_hardware_model) {
        case HardwareModel::A8:
            minimum_ver.camera.major = 0;
            minimum_ver.camera.minor = 2;
            minimum_ver.camera.patch = 1;
            break;

        case HardwareModel::A2:
        case HardwareModel::ZR10:
        case HardwareModel::ZR30:
        case HardwareModel::ZT6:
        case HardwareModel::ZT30:
            // TBD
            break;

        case HardwareModel::UNKNOWN:
            printf("Can't check FW on unknown hardware model!\n");
            return;
    }

    const uint32_t minimum_camera_val =  (minimum_ver.camera.major << 16) + (minimum_ver.camera.minor << 8) + minimum_ver.camera.patch;
    const uint32_t firmware_camera_val = (_fw_version.camera.major << 16) + (_fw_version.camera.minor << 8) + _fw_version.camera.patch;

    const bool is_camera_supported = firmware_camera_val >= minimum_camera_val;

    if (!is_camera_supported) {
        printf(
            "Mount: Siyi running old camera fw (need v%u.%u.%u)",
            minimum_ver.camera.major, minimum_ver.camera.minor, minimum_ver.camera.patch
        );
    }
}


void MountSiyi::handlerRecv()
{
	int16_t nbytes;
	uint8_t buf[512];

	//wait for _initialised = true
	sleepMs(100);

	// 100hz
	while(_initialised)
	{
		sleepMs(10);

		do
		{
			nbytes =0;
			bzero(buf, sizeof(buf));

			if(m_interfaceType == SIYI_TCP && m_tcpSockfd > 0)
			{
				nbytes = recv(m_tcpSockfd, buf ,sizeof(buf), 0);

				if(nbytes < 0)
				{
					printf("tcp recv error \n");
 					close(m_tcpSockfd);
					m_tcpSockfd = -1;
				}
			}
			else if(m_interfaceType == SIYI_UDP && m_udpSockfd > 0)
			{
				socklen_t addr_len = sizeof(struct sockaddr_in);
				nbytes = recvfrom(m_udpSockfd, buf, sizeof(buf), 0, (struct sockaddr *)&m_saddr, &addr_len);

				if(nbytes < 0)
				{
					printf("udp recv error \n");
 					close(m_udpSockfd);
					m_udpSockfd = -1;
				}
			}

			if(nbytes > 0)
			{
				//printf("recv nbytes:%d\n", nbytes);
				read_incoming_packets(buf, nbytes);
			}
		}while(nbytes == sizeof(buf));
	}
}


void MountSiyi::handlerSend()
{
	int len;
	uint8_t buf[512];
	//wait for _initialised = true
	sleepMs(100);

	//run 100 hz
	while(_initialised)
	{
		sleepMs(10);

		do
		{
			len =0;
			bzero(buf, sizeof(buf));
			len = readCircularBuf(&m_sendBuffer, buf, sizeof(buf));

			if(len > 0)
			{
				int ret;
				if(m_interfaceType == SIYI_TCP && m_tcpSockfd > 0)
				{
					ret = send(m_tcpSockfd, buf, len, MSG_NOSIGNAL);
					if(ret < 0)
					{
						printf("tcp send error\n");
						close(m_tcpSockfd);
						m_tcpSockfd = -1;
					}
				}
				else if(m_interfaceType == SIYI_UDP && m_udpSockfd > 0)
				{
					//printf("sendto %d\n", len);
					ret = sendto(m_udpSockfd, buf, len, 0, (struct sockaddr *)&m_saddr, sizeof(m_saddr));
					if(ret < 0)
					{
						printf("udp send error\n");
						close(m_udpSockfd);
						m_udpSockfd = -1;
					}
				}
			}
		}while(len == sizeof(buf));
	}
}

void *siyiRecvRun(void *arg)
{
	mountSiyiHandler.handlerRecv();
	return NULL;
}

void *siyiSendRun(void *arg)
{
	mountSiyiHandler.handlerSend();
	return NULL;
}

bool MountSiyi::tcpClient(const char *ip, const char *port)
{
	m_tcpSockfd=socket(AF_INET,SOCK_STREAM,0);
	if(m_tcpSockfd < 0)
	{
		printf("tcp socket error\n");
		return false;
	}

	memset(&m_saddr,0,sizeof(m_saddr));
	m_saddr.sin_family=AF_INET;
	m_saddr.sin_port=htons(atoi(port));
	m_saddr.sin_addr.s_addr=inet_addr(ip);

	int ret =connect(m_tcpSockfd,(struct sockaddr*)&m_saddr,sizeof(m_saddr));
	if(ret < 0)
	{
		close(m_tcpSockfd);
		m_tcpSockfd = -1;
		printf("tcp connect error\n");
		return false;
	}

	ret = pthread_create(&m_RecvRunTid, NULL, siyiRecvRun, NULL);
	if(0 != ret)
	{
		close(m_tcpSockfd);
		m_tcpSockfd = -1;
		printf("tcp recv pthread_create err\n");
		return false;
	}

	ret = pthread_create(&m_SendRunTid, NULL, siyiSendRun, NULL);
	if(0 != ret)
	{
		close(m_tcpSockfd);
		m_tcpSockfd = -1;
		printf("tcp send  pthread_create err\n");
		return false;
	}

	return true;
}


bool MountSiyi::udpClient(const char *ip, const char *port)
{
	m_udpSockfd=socket(AF_INET,SOCK_DGRAM,0);
	if(m_udpSockfd < 0)
	{
		printf("udp socket error\n");
		return false;
	}

	memset(&m_saddr,0,sizeof(m_saddr));
	m_saddr.sin_family=AF_INET;
	m_saddr.sin_port=htons(atoi(port));
	m_saddr.sin_addr.s_addr=inet_addr(ip);

	int ret;
	ret = pthread_create(&m_RecvRunTid, NULL, siyiRecvRun, NULL);
	if(0 != ret)
	{
		close(m_udpSockfd);
		m_udpSockfd = -1;
		printf("udp recv pthread_create err\n");
		return false;
	}

	ret = pthread_create(&m_SendRunTid, NULL, siyiSendRun, NULL);
	if(0 != ret)
	{
		close(m_udpSockfd);
		m_udpSockfd = -1;
		printf("udp send  pthread_create err\n");
		return false;
	}

	return true;
}


bool MountSiyi::set_motion_mode_lock()
{
	return set_motion_mode(GimbalMotionMode::LOCK, true);
}

bool MountSiyi::set_motion_mode_follow()
{
	return set_motion_mode(GimbalMotionMode::FOLLOW, true);
}

bool MountSiyi::set_motion_mode_fpv()
{
	return set_motion_mode(GimbalMotionMode::FPV, true);
}

MountSiyi mountSiyiHandler;
